/**
 * LaserServoScanner.pde --
 *
 * 2009, Tod E. Kurt, http://todbot.com/blog/
 *
 */

#include <Servo.h> 
 
Servo servo1; 
Servo servo2; 
//Servo servo3; 

int pos;
//int posinc = 1;  // +1 or -1
int delaymillis = 800;

void setup() 
{ 
  servo1.attach(7); 
  servo2.attach(6); 
  //servo3.attach(6); 
  pos = 90;
  
} 
 
void loop() 
{ 
  pos = (pos==180) ? 0 : 180;

  servo1.write( pos );
  servo2.write( pos );    
  //servo3.write( pos );    
  
  delay(delaymillis);
}

// return new pos from current pos, dest and rate
int slew_servo( int pos, int dest, int rate) 
{
    int diff = pos - dest;
    if( diff == 0 ) return pos;
    else if( diff > -rate ) pos++;
    else if( diff <  rate ) pos--;
    if(      diff > 0 ) pos -= rate;
    else if( diff < 0 ) pos += rate;
    return pos;
}

